#include "stdafx.h"

#include "gp.h"
#include "gp_Dir.h"
#include "gp_Pnt.h"
#include "gp_Trsf.h"
#include "gp_Dir.h"
#include "gp_XYZ.h"
#include "gp_Pnt.h"
#include "gp_Ax1.h"
#include "gp_Ax2.h"
#include "gp_Trsf.h"
#include "gp_Vec.h"


bool gp_Vec::IsEqual
	(const gp_Vec& Other, 
	const double LinearTolerance,
	const double AngularTolerance) const
{
	if (Magnitude ()       <= LinearTolerance || 
		Other.Magnitude () <= LinearTolerance) {
			double val = Magnitude() - Other.Magnitude();
			if (val < 0) val = - val;
			return val <= LinearTolerance;
	}
	else {
		double val = Magnitude() - Other.Magnitude();
		if (val < 0) val = - val;
		return val <= LinearTolerance && Angle(Other) <= AngularTolerance;
	}
}    

void gp_Vec::Mirror (const gp_Vec& V)
{
	double D = V.coord.Modulus();
	if (D > gp::Resolution()) {
		const gp_XYZ& XYZ = V.coord;
		double A = XYZ.X() / D;
		double B = XYZ.Y() / D;
		double C = XYZ.Z() / D; 
		double M1 = 2.0 * A * B;
		double M2 = 2.0 * A * C;
		double M3 = 2.0 * B * C;
		double X = coord.X();
		double Y = coord.Y();
		double Z = coord.Z();
		coord.SetX(((2.0 * A * A) - 1.0) * X + M1 * Y + M2 * Z);
		coord.SetY(M1 * X + ((2.0 * B * B) - 1.0) * Y + M3 * Z);
		coord.SetZ(M2 * X + M3 * Y + ((2.0 * C * C) - 1.0) * Z);
	}
}

void gp_Vec::Mirror (const gp_Ax1& A1)
{
	const gp_XYZ& V = A1.Direction().XYZ();
	double A = V.X();
	double B = V.Y();
	double C = V.Z();
	double X = coord.X();
	double Y = coord.Y();
	double Z = coord.Z();
	double M1 = 2.0 * A * B;
	double M2 = 2.0 * A * C;
	double M3 = 2.0 * B * C;
	coord.SetX(((2.0 * A * A) - 1.0) * X + M1 * Y + M2 * Z);
	coord.SetY(M1 * X + ((2.0 * B * B) - 1.0) * Y + M3 * Z);
	coord.SetZ(M2 * X + M3 * Y + ((2.0 * C * C) - 1.0) * Z);
}

void gp_Vec::Mirror (const gp_Ax2& A2)
{
	gp_XYZ Z      = A2.Direction().XYZ();
	gp_XYZ MirXYZ = Z.Crossed (coord);
	if (MirXYZ.Modulus() <= gp::Resolution()) { coord.Reverse(); }
	else {
		Z.Cross (MirXYZ);
		Mirror (Z);
	}
}

void gp_Vec::Transform(const gp_Trsf& T)
{
	if (T.Form() == gp_Identity || T.Form() == gp_Translation) { }
	else if (T.Form() == gp_PntMirror) { coord.Reverse(); }
	else if (T.Form() == gp_Scale) { coord.Multiply (T.ScaleFactor()); }
	else { coord.Multiply (T.VectorialPart()); }
}

gp_Vec gp_Vec::Mirrored (const gp_Vec& V) const
{
	gp_Vec Vres = *this;
	Vres.Mirror (V);
	return Vres;                     
}

gp_Vec gp_Vec::Mirrored (const gp_Ax1& A1) const
{
	gp_Vec Vres = *this;
	Vres.Mirror (A1);
	return Vres;                     
}

gp_Vec gp_Vec::Mirrored (const gp_Ax2& A2) const
{
	gp_Vec Vres = *this;
	Vres.Mirror (A2);
	return Vres;                     
}
 gp_Vec::gp_Vec() { }

 gp_Vec::gp_Vec (const gp_Dir& V) { coord = V.XYZ(); }

 gp_Vec::gp_Vec (const gp_XYZ& Coord) : coord(Coord) { }

 gp_Vec::gp_Vec (const double Xv,
		       const double Yv,
		       const double Zv)
               : coord (Xv, Yv, Zv)   { }

 gp_Vec::gp_Vec (const gp_Pnt& P1,
		       const gp_Pnt& P2)
{ coord = P2.XYZ().Subtracted(P1.XYZ()); }

 void gp_Vec::SetCoord (const int Index,
			      const double Xi)
{ coord.SetCoord (Index, Xi); }

 void gp_Vec::SetCoord (const double Xv,
			      const double Yv,
			      const double Zv)
{
  coord.SetX(Xv);
  coord.SetY(Yv);
  coord.SetZ(Zv);
}

 void gp_Vec::SetX (const double X)
{ coord.SetX (X); }

 void gp_Vec::SetY (const double Y)
{ coord.SetY (Y); }

 void gp_Vec::SetZ (const double Z)
{ coord.SetZ (Z); }

 void gp_Vec::SetXYZ (const gp_XYZ& Coord)
{ coord = Coord; }

 double gp_Vec::Coord (const int Index) const
{ return coord.Coord (Index); }

 void gp_Vec::Coord(double& Xv, 
			  double& Yv,
			  double& Zv) const
{
  Xv = coord.X();
  Yv = coord.Y();
  Zv = coord.Z();
}

 double gp_Vec::X() const
{ return coord.X(); }
     
 double gp_Vec::Y() const
{ return coord.Y(); }

 double gp_Vec::Z() const
{ return coord.Z(); }
     
 const gp_XYZ& gp_Vec::XYZ () const
{ return coord; }

 bool gp_Vec::IsNormal
(const gp_Vec& Other,
 const double AngularTolerance) const
{
  double Ang = M_PI / 2.0 - Angle(Other);
  if (Ang < 0) Ang = - Ang;
  return  Ang <= AngularTolerance;
}    

 bool gp_Vec::IsOpposite
(const gp_Vec& Other,
 const double AngularTolerance) const
{
  double Ang = M_PI - Angle(Other);
  return Ang <= AngularTolerance;
}    

 bool gp_Vec::IsParallel
(const gp_Vec& Other,
 const double AngularTolerance) const
{
  double Ang = Angle (Other);
  return   Ang <= AngularTolerance || M_PI - Ang <= AngularTolerance;
}    

 double gp_Vec::Angle (const gp_Vec& Other) const
{
  //    Commentaires :
  //    Au dessus de 45 degres l'arccos donne la meilleur precision pour le
  //    calcul de l'angle. Sinon il vaut mieux utiliser l'arcsin.
  //    Les erreurs commises sont loin d'etre negligeables lorsque l'on est
  //    proche de zero ou de 90 degres.
  //    En 3d les valeurs angulaires sont toujours positives et comprises entre
  //    0 et Pi.
  
	gp_VectorWithNullMagnitude_Raise_if
	(coord.Modulus() <= gp::Resolution() ||
		Other.coord.Modulus() <= gp::Resolution(), " ");
  return (gp_Dir(coord)).Angle(Other);
}

 double gp_Vec::AngleWithRef (const gp_Vec& Other,
					   const gp_Vec& Vref) const
{
	 gp_VectorWithNullMagnitude_Raise_if
	 (coord.Modulus() <= gp::Resolution() ||
		 Vref.coord.Modulus() <= gp::Resolution() ||
		 Other.coord.Modulus() <= gp::Resolution(), " ");
  return (gp_Dir(coord)).AngleWithRef(Other,Vref);
} 

 double gp_Vec::Magnitude() const
{ return coord.Modulus(); }

 double gp_Vec::SquareMagnitude() const
{ return coord.SquareModulus(); }

 void gp_Vec::Add (const gp_Vec& Other)
{ coord.Add (Other.coord); }

 gp_Vec gp_Vec::Added (const gp_Vec& Other) const
{
  gp_Vec V = *this;
  V.coord.Add (Other.coord);
  return V;
}

 void gp_Vec::Subtract (const gp_Vec& Right)
{ coord.Subtract (Right.coord); }

 gp_Vec gp_Vec::Subtracted (const gp_Vec& Right) const
{
  gp_Vec V = *this;
   V.coord.Subtract(Right.coord);
   return V;
}

 void gp_Vec::Multiply (const double Scalar)
{ coord.Multiply(Scalar); }

 gp_Vec gp_Vec::Multiplied (const double Scalar) const
{
  gp_Vec V = *this;
  V.coord.Multiply (Scalar);
  return V;
}

 void gp_Vec::Divide (const double Scalar)
{ coord.Divide (Scalar); }

 gp_Vec gp_Vec::Divided (const double Scalar) const
{
  gp_Vec V = *this;
  V.coord.Divide (Scalar);
  return V;
}

 void gp_Vec::Cross (const gp_Vec& Right)
{ coord.Cross (Right.coord); }

 gp_Vec gp_Vec::Crossed (const gp_Vec& Right) const
{
  gp_Vec V = *this;
  V.coord.Cross (Right.coord);
  return V;
}

 double gp_Vec::CrossMagnitude
(const gp_Vec& Right) const
{ return coord.CrossMagnitude (Right.coord); }

 double gp_Vec::CrossSquareMagnitude
(const gp_Vec& Right) const
{ return coord.CrossSquareMagnitude (Right.coord); }

 void gp_Vec::CrossCross (const gp_Vec& V1,
				const gp_Vec& V2)
{ coord.CrossCross(V1.coord, V2.coord); }

 gp_Vec gp_Vec::CrossCrossed (const gp_Vec& V1,
				    const gp_Vec& V2) const
{
  gp_Vec V = *this;
  V.coord.CrossCross(V1.coord, V2.coord);
  return V;
}

 double gp_Vec::Dot (const gp_Vec& Other) const
{ return coord.Dot (Other.coord); }

 double gp_Vec::DotCross (const gp_Vec& V1,
				       const gp_Vec& V2)  const
{ return coord.DotCross (V1.coord, V2.coord); } 

 void gp_Vec::Normalize()
{ 
  double D = coord.Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.Divide (D);
}

 gp_Vec gp_Vec::Normalized() const
{ 
  double D = coord.Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  gp_Vec V = *this;
  V.coord.Divide (D);
  return V; 
}

 void gp_Vec::Reverse()
{ coord.Reverse(); }

 gp_Vec gp_Vec::Reversed () const
{
  gp_Vec V = *this;
  V.coord.Reverse();
  return V;
}

 void gp_Vec::SetLinearForm
(const double L,
 const gp_Vec& Left,
 const double R,
 const gp_Vec& Right)
{ coord.SetLinearForm (L, Left.coord, R, Right.coord); }

 void gp_Vec::SetLinearForm
(const double L,
 const gp_Vec& Left,
 const gp_Vec& Right)
{ coord.SetLinearForm (L, Left.coord, Right.coord); }

 void gp_Vec::SetLinearForm
(const gp_Vec& Left,
 const gp_Vec& Right)
{ coord.SetLinearForm (Left.coord,  Right.coord); }

 void gp_Vec::SetLinearForm
(const double A1, const gp_Vec& V1, 
 const double A2, const gp_Vec& V2,
 const double A3, const gp_Vec& V3)
{ coord.SetLinearForm (A1, V1.coord, A2, V2.coord, A3, V3.coord); }

 void gp_Vec::SetLinearForm
(const double A1, const gp_Vec& V1, 
 const double A2, const gp_Vec& V2, 
 const gp_Vec& V3)
{ coord.SetLinearForm (A1, V1.coord, A2, V2.coord, V3.coord); }

 void gp_Vec::SetLinearForm
(const double A1, const gp_Vec& V1,
 const double A2, const gp_Vec& V2,
 const double A3, const gp_Vec& V3,
 const gp_Vec& V4)
{ coord.SetLinearForm(A1,V1.coord,A2,V2.coord,A3,V3.coord,V4.coord); }

 void gp_Vec::Rotate (const gp_Ax1& A1,
			    const double Ang)
{
  gp_Trsf T;
  T.SetRotation (A1, Ang);
  coord.Multiply (T.VectorialPart());
}

 gp_Vec gp_Vec::Rotated (const gp_Ax1& A1,
			       const double Ang) const
{
  gp_Vec Vres = *this;
  Vres.Rotate (A1, Ang);
  return Vres;                     
}

 void gp_Vec::Scale (const double S)
{ coord.Multiply (S); }

 gp_Vec gp_Vec::Scaled (const double S) const
{
  gp_Vec V = *this;
  V.coord.Multiply(S);
  return V;
}

 gp_Vec gp_Vec::Transformed (const gp_Trsf& T) const
{
  gp_Vec V = *this;
  V.Transform(T);
  return V;
} 

 gp_Vec operator* (const double Scalar, const gp_Vec& V) {
 return V.Multiplied(Scalar);
}

